#include "hikari.h"
#include "astar.h"
void RegionConnection::removeConnectionPoints(){
	int count=connectionPoints.size();
	for(int i=0;i<count;i++){
		delete connectionPoints[i];
		connectionPoints[i]=NULL;
	}

	connectionPoints.clear();
}

void NavmeshRegion::createCells(){
	int count=xCount*yCount;
	cells = new Cell*[count];
	for (int i = 0; i < count; i++) {
		cells[i]=new Cell();
	}
}

void NavmeshRegion::removeCells(){
	int count=xCount*yCount;
	for (int i = 0; i < count; i++) {
		//todo: As navmeshes share cells, one navmeshRegion can delete cell of another navmeshRegion, and it's will cause crash
		delete cells[i];
		cells[i]=NULL;
	}

	delete [] cells;
	cells=NULL;
}

void NavmeshRegion::clearConnections(){
	for(std::hash_map<NavmeshRegion*,RegionConnection*>::iterator it = connectionMap.begin(); it != connectionMap.end(); ++it) {
		delete it->second;
		it->second=NULL;
	}

	connectionMap.clear();
}

Cell* NavmeshRegion::getCell(unsigned int x, unsigned int y){
	if(x<0 || y<0 || x>=xCount ||y>yCount){
		Utils::errorLog(wstring(L"Cannot get cell with [x,y]=[")
			+Utils::wstring(x)+wstring(L",")+Utils::wstring(y)
			+wstring(L"] but boundary is [")
			+Utils::wstring(xCount)+wstring(L",")+Utils::wstring(yCount)
			+wstring(L"]"));
		return NULL;
	}

	unsigned int index=y*xCount+x;
	Cell*cell=cells[index];
	return cell;
}

void NavmeshRegion::setCell(unsigned int x, unsigned int y, Cell*cell){
	if(x<0 || y<0 || x>=xCount ||y>yCount){
		Utils::errorLog(wstring(L"Cannot get cell with [x,y]=[")
			+Utils::wstring(x)+wstring(L",")+Utils::wstring(y)
			+wstring(L"] but boundary is [")
			+Utils::wstring(xCount)+wstring(L",")+Utils::wstring(yCount)
			+wstring(L"]"));
	}

	unsigned int index=y*xCount+x;
	cells[index]=cell;
}

void NavmeshRegion::addConnection(NavmeshRegion*region, RegionConnection*connection){
	connection->setConnectedRegion(region);
	connectionMap[region]=connection;
}

bool NavmeshRegion::IsGoal( NavmeshRegion*nodeGoal ){
	return this->getId()==nodeGoal->getId();
}

bool NavmeshRegion::GetSuccessors( astarPointer::AStarSearch<NavmeshRegion*> *astarsearch, NavmeshRegion **parent_nodePointer ){
	NavmeshRegion*parentNode=NULL;
	if(parent_nodePointer!=NULL){
		parentNode=*parent_nodePointer;
	}
	for(std::hash_map<NavmeshRegion*,RegionConnection*>::iterator it = connectionMap.begin(); it != connectionMap.end(); ++it) {
		NavmeshRegion*region=it->first;this;
		
		if(region!=parentNode){
			astarsearch->AddSuccessor(region);
		}
	}

	return true;
}

float NavmeshRegion::GoalDistanceEstimate( NavmeshRegion *nodeGoal ){
	float xd=(float)x - (float)nodeGoal->x;
	float yd=(float)y - (float)nodeGoal->y;
	return (float)(x+y);
}

float NavmeshRegion::GetCost( NavmeshRegion*successor ){
	return 1.0f;
}

bool NavmeshRegion::IsSameState( NavmeshRegion*rhs ) {
	bool result=this->getId()==rhs->getId();
	return result;
}

bool MapSearchNode::IsSameState( MapSearchNode &rhs ) {
	// same state in a maze search is simply when (x,y) are the same
	if( (x == rhs.x) && (y == rhs.y) ) {
		return true;
	} else {
		return false;
	}
}

bool NavmeshRegion::isActiveCellFast(int x, int y) {
	Cell*gc = cells[y*xCount+x];
	return gc->isWalkable();
}

bool NavmeshRegion::checkVisionLine(int x1, int y1, int x2, int y2) {
	int ystep, xstep;    // the step on y and x axis 
	int error;           // the error accumulated during the increment 
	int errorprev;       // *vision the previous value of the error variable 
	int ty = y1, tx = x1;  // the line points 
	int ddy, ddx;        // compulsory variables: the double values of dy and dx 
	int dx = x2 - x1;
	int dy = y2 - y1;
	if (!isActiveCellFast(x1, y1)) {
		return false;
	}

	// NB the last point can't be here, because of its previous point (which has to be verified) 
	if (dy < 0) {
		ystep = -1;
		dy = -dy;
	} else {
		ystep = 1;
	}
	if (dx < 0) {
		xstep = -1;
		dx = -dx;
	} else {
		xstep = 1;
	}

	ddy = 2 * dy;  // work with double values for full precision 
	ddx = 2 * dx;
	if (ddx >= ddy) {  // first octant (0 <= slope <= 1) 
		// compulsory initialization (even for errorprev, needed when dx==dy) 
		errorprev = error = dx;  // start in the middle of the square 
		for (int i = 0; i < dx; i++) {  // do not use the first point (already done) 
			tx += xstep;
			error += ddy;
			if (error > ddx) {  // increment y if AFTER the middle ( > ) 
				ty += ystep;
				error -= ddx;
				// three cases (octant == right->right-top for directions below): 
				if (error + errorprev < ddx) // bottom square also 
				{
					if (!isActiveCellFast(tx, ty - ystep)) {
						return false;
					}
				} else if (error + errorprev > ddx) // left square also 
				{
					if (!isActiveCellFast(tx - xstep, ty)) {
						return false;
					}
				} else {  // corner: bottom and left squares also 
					if (!isActiveCellFast(tx, ty - ystep)) {
						return false;
					}
					if (!isActiveCellFast(tx - xstep, ty)) {
						return false;
					}
				}
			}

			if (!isActiveCellFast(tx, ty)) {
				return false;
			}

			errorprev = error;
		}
	} else {  // the same as above 
		errorprev = error = dy;
		for (int i = 0; i < dy; i++) {
			ty += ystep;
			error += ddx;
			if (error > ddy) {
				tx += xstep;
				error -= ddy;
				if (error + errorprev < ddy) {
					if (!isActiveCellFast(tx - xstep, ty)) {
						return false;
					}
				} else if (error + errorprev > ddy) {
					if (!isActiveCellFast(tx, ty - ystep)) {
						return false;
					}
				} else {
					if (!isActiveCellFast(tx - xstep, ty)) {
						return false;
					}
					if (!isActiveCellFast(tx, ty - ystep)) {
						return false;
					}
				}
			}

			if(tx==10 && ty==4){
			//TODO: debug remove
				int x=5;x++;
			}

			if (!isActiveCellFast(tx, ty)) {
				return false;
			}

			errorprev = error;
		}
	}

	return true;
}

// Here's the heuristic function that estimates the distance from a Node
// to the Goal. 
float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal ) {
	float xd = float( ( (float)x - (float)nodeGoal.x ) );
	float yd = float( ( (float)y - (float)nodeGoal.y) );
	return xd + yd;
}

bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal ) {
	if( (x == nodeGoal.x) && (y == nodeGoal.y) ) {
		return true;
	}

	return false;
}

// This generates the successors to the given Node. It uses a helper function called
// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
// is done for each node internally, so here you just set the state information that
// is specific to the application
bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node ) {
	int parent_x = -1; 
	int parent_y = -1; 
	if( parent_node ) {
		parent_x = parent_node->x;
		parent_y = parent_node->y;
	}

	MapSearchNode NewNode;

	// push each possible move except allowing the search to go backwards
	int tx;
	int ty;
	int xcount=currentRegion->getxCount();
	int ycount=currentRegion->getyCount();
	Cell*cell;

	tx=x-1;
	ty=y;
	if(tx>0 || ty>0 || tx<xcount || ty<ycount){
		cell=currentRegion->getCell(tx, ty);
		if(cell->isWalkable() 
			&& !((parent_x == tx) && (parent_y == ty))) {
				NewNode = MapSearchNode( tx, ty, currentRegion);
				astarsearch->AddSuccessor( NewNode );
		}        
	}

	tx=x;
	ty=y-1;
	if(tx>0 || ty>0 || tx<xcount || ty<ycount){
		cell=currentRegion->getCell(tx, ty);
		if(cell->isWalkable() 
			&& !((parent_x == tx) && (parent_y == ty))) {
				NewNode = MapSearchNode( tx, ty, currentRegion);
				astarsearch->AddSuccessor( NewNode );
		}        
	}        

	tx=x+1;
	ty=y;
	if(tx>0 || ty>0 || tx<xcount || ty<ycount){
		cell=currentRegion->getCell(tx, ty);
		if(cell->isWalkable() 
			&& !((parent_x == tx) && (parent_y == ty))) {
				NewNode = MapSearchNode( tx, ty, currentRegion);
				astarsearch->AddSuccessor( NewNode );
		}        
	}

	tx=x;
	ty=y+1;
	if(tx>0 || ty>0 || tx<xcount || ty<ycount){
		cell=currentRegion->getCell(tx, ty);
		if(cell->isWalkable() 
			&& !((parent_x == tx) && (parent_y == ty))) {
				NewNode = MapSearchNode( tx, ty, currentRegion);
				astarsearch->AddSuccessor( NewNode );
		}        
	}

	return true;
}

// given this node, what does it cost to move to successor. In the case
// of our map the answer is the map terrain value at this node since that is 
// conceptually where we're moving

float MapSearchNode::GetCost( MapSearchNode &successor ){
	return 1;

}

void NavmeshManager::clearNavmeshInfo(){
	int count=getRegions().size();
	for(int i=0;i<count;i++){
		delete getRegions()[i];
		getRegions()[i]=NULL;
	}

	getRegions().clear();
}

void draw2dRect(IVideoDriver*driver, vector3df*pos, float width, float height){
	float x1=(float)(pos->X);
	float y1=(float)(pos->Z);

	float x2=(float)(x1+width);
	float y2=(float)(y1+height);
	driver->draw3DLine(vector3df(x1,2,y1), vector3df(x2,2,y1));
	driver->draw3DLine(vector3df(x1,2,y1), vector3df(x1,2,y2));
	driver->draw3DLine(vector3df(x2,2,y2), vector3df(x2,2,y1));
	driver->draw3DLine(vector3df(x2,2,y2), vector3df(x1,2,y2));
}

void NavmeshManager::drawNavmesh(irr::video::IVideoDriver*driver){
	driver->setTransform(irr::video::ETS_WORLD, irr::core::IdentityMatrix);
	
	/*irr::video::SMaterial inactivematerial;
	inactivematerial.EmissiveColor.setRed(255);
	inactivematerial.EmissiveColor.setGreen(0);*/
	irr::video::SMaterial activematerial;
	activematerial.EmissiveColor.setGreen(255);
	activematerial.EmissiveColor.setRed(0);

	irr::video::SMaterial startEndMaterial;
	startEndMaterial.EmissiveColor.setGreen(10);
	startEndMaterial.EmissiveColor.setRed(10);
	startEndMaterial.EmissiveColor.setBlue(255);

	int count=getRegions().size();
	for(int i=0;i<count;i++){
		NavmeshRegion*region=getRegions()[i];
		double cellWidth=region->getWidth()/region->getxCount();
		double cellHeight=region->getHeight()/region->getyCount();
		for(unsigned int j=0;j<region->getyCount();j++){
			for(unsigned int k=0;k<region->getxCount();k++){
				float x1=(float)(region->getX()+cellWidth*k+0.1);
				float y1=(float)(region->getY()+cellHeight*j+0.1);

				float x2=(float)(x1+cellWidth-0.2);
				float y2=(float)(y1+cellHeight-0.2);
				bool active=region->getCell(k,j)->isWalkable();
				if(active){
					driver->setMaterial(activematerial);
					driver->draw3DLine(vector3df(x1,0.5f,y1), vector3df(x2,0.5f,y1));
					driver->draw3DLine(vector3df(x1,0.5f,y1), vector3df(x1,0.5f,y2));
					driver->draw3DLine(vector3df(x2,0.5f,y2), vector3df(x2,0.5f,y1));
					driver->draw3DLine(vector3df(x2,0.5f,y2), vector3df(x1,0.5f,y2));
				}
			}
			//printf("\n");
		}

		if(drawLocalStart!=NULL){
			driver->setMaterial(startEndMaterial);
			draw2dRect(driver, drawLocalStart, cellWidthHeight, cellWidthHeight);
		}

		if(drawLocalEnd!=NULL){
			driver->setMaterial(startEndMaterial);
			draw2dRect(driver, drawLocalEnd, cellWidthHeight, cellWidthHeight);
		}
	}
}

NavmeshRegion*NavmeshManager::getRegion(float x, float y){
	int count=getRegions().size();
	for(int i=0;i<count;i++){
		NavmeshRegion*region=getRegions()[i];
		if(region->pointInNavmesh(x, y)){
			return region;
		}
	}

	return NULL;
}

void RegionConnection::readConnectionPoints(FileSection*section){
	removeConnectionPoints();
	vector<int>*points=section->readInts(section->getChunk(L"cPoints"));
	if(points==NULL || points->size()==0){
		return;
	}

	int connectionPointsCount=points->size()/4;
	for(int i=0;i<connectionPointsCount;i++){
		ConnectionPoint*point=new ConnectionPoint();
		int index=i*4;
		point->setPoint1((*points)[index+0], (*points)[index+1]);
		point->setPoint2((*points)[index+2], (*points)[index+3]);
		addConnectionPoint(point);
	}

	delete points;
	points=NULL;
}

void removeDuplicatesInRegions(NavmeshRegion*region1, NavmeshRegion*region2, vector<ConnectionPoint*>&connectedPoints){
	for(unsigned int i=0;i<connectedPoints.size();i++){
		ConnectionPoint*point=connectedPoints[i];
		int tx=point->getPoint1().getX();
		int ty=point->getPoint1().getY();
		Cell*cell1=region1->getCell(tx, ty);
		tx=point->getPoint2().getX();
		ty=point->getPoint2().getY();
		Cell*cell2=region2->getCell(tx, ty);
		if(cell1!=cell2){
			tx=point->getPoint2().getX();
			ty=point->getPoint2().getY();
			region2->setCell(tx, ty, cell1);
			delete cell2;
		}
	}
}

void NavmeshManager::removeDuplicatedConnectedCells(){
	for(unsigned int i=0;i<regions.size();i++){
		NavmeshRegion*region=regions[i];
		for(stdext::hash_map<NavmeshRegion*,RegionConnection*>::iterator it = region->getConnections().begin(); it != region->getConnections().end(); ++it) {
			NavmeshRegion*connectedRegion=it->second->getRegion();
			vector<ConnectionPoint*>connectedPoints=it->second->getConnectionPoints();
			removeDuplicatesInRegions(region, connectedRegion, connectedPoints);
		}
	}
}

void NavmeshManager::unload(){
	int size=this->getRegions().size();
	for(int i=0;i<size;i++){
		delete this->getRegions()[i];
	}

	this->getRegions().clear();
}

void NavmeshManager::load(FileSection*section){
	clearNavmeshInfo();
	int regionsCount=section->readInt(section->getChunk(L"cellsCount"));
	hash_map<int, NavmeshRegion*>intCellMap;
	for(int j=0;j<regionsCount;j++){
		NavmeshRegion*cell=new NavmeshRegion();
		cell->setId(j+1);
		FileSection*cellSection=section->readSection(section->getChunk(wstring(L"cell") + Utils::wstring(j)));
		int cellId=cellSection->readInt(cellSection->getChunk(L"cellId"));
		intCellMap[cellId]=cell;
		cell->setxCount(cellSection->readInt(cellSection->getChunk(L"cellWidth")));
		cell->setyCount(cellSection->readInt(cellSection->getChunk(L"cellHeight")));

		cell->setX(cellSection->readFloat(cellSection->getChunk(L"x")));
		cell->setY(cellSection->readFloat(cellSection->getChunk(L"y")));
		cell->setWidth(cellSection->readFloat(cellSection->getChunk(L"width")));
		cell->setHeight(cellSection->readFloat(cellSection->getChunk(L"height")));
		cell->createCells();

		vector<bool>*values=cellSection->readBooleans(cellSection->getChunk(L"cellValues"));
		int index=0;
		//printf("Region %d load\n", cellId);
		for(unsigned int k=0;k<cell->getyCount();k++){
			for(unsigned int i=0;i<cell->getxCount();i++){
				bool value=(*values)[index];
			//	printf("%d", value?1:0);
				cell->getCell(i,k)->setActive(value);
				index++;
			}
			//printf("\n");
		}
		//TODO:debug
		//printf("\n\n\n");

		getRegions().push_back(cell);

		delete values;
		values=NULL;

		delete cellSection;
		cellSection=NULL;
	}

	//second we read connections between sections
	for (int j = 0; j < regionsCount; j++) {
		FileSection*cellSection = section->readSection(section->getChunk(wstring(L"cell") + Utils::wstring(j)));
		int cellId=cellSection->readInt(cellSection->getChunk(L"cellId"));
		int connectedCellCount = cellSection->readInt(cellSection->getChunk(L"connectedCellsCount"));
		NavmeshRegion*currentCell=intCellMap[cellId];
		for (int i = 0; i < connectedCellCount; i++) {
			RegionConnection*connection=new RegionConnection();
			FileSection*connectedCellSection = cellSection->readSection(cellSection->getChunk(wstring(L"connCell") + Utils::wstring(i)));
			int connectedRegionId=connectedCellSection->readInt(connectedCellSection->getChunk(L"connectedCellId"));
			NavmeshRegion*connectedRegion=intCellMap[connectedRegionId];
			connection->readConnectionPoints(connectedCellSection);
			currentCell->addConnection(connectedRegion, connection);
			delete connectedCellSection;
			connectedCellSection=NULL;
		}

		delete cellSection;
		cellSection=NULL;
	}

	removeDuplicatedConnectedCells();
}

/**
** In this method we only try to find basic "interregions" path. Local "in-region-path" will be searched in NavmeshPath class
*/
NavmeshPath*NavmeshManager::generatePath(vector3df&startPosition, vector3df&endPosition){
	NavmeshPath*path=new NavmeshPath(this);
	path->getStartPoint().setXY(startPosition.X,startPosition.Z);
	path->getEndPoint().setXY(endPosition.X,endPosition.Z);
	path->getStartLocalPoint().setXY(-1,-1);
	path->getEndLocalPoint().setXY(-1,-1);
	NavmeshRegion*startSearchRegion=getRegion(startPosition.X, startPosition.Z);
	NavmeshRegion*endSearchRegion=getRegion(endPosition.X, endPosition.Z);
	if(startSearchRegion==NULL || endSearchRegion==NULL){
		delete path;
		return NULL;
	}

	if(startSearchRegion==endSearchRegion){
		path->getRegionsPath().push_back(startSearchRegion);
	}else{
		astarPointer::AStarSearch<NavmeshRegion*>astarsearcher;
		astarsearcher.SetStartAndGoalStates(startSearchRegion, endSearchRegion);
		bool result=astarsearcher.search();
		if(result){
			NavmeshRegion**startSolutionRegionPointer=astarsearcher.GetSolutionStart();
			NavmeshRegion*startSolutionRegion=*startSolutionRegionPointer;
			path->getRegionsPath().push_back(startSearchRegion);
			for(;;){
				NavmeshRegion**nodePointer= astarsearcher.GetSolutionNext();
				if( !nodePointer ){
					break;
				}
				NavmeshRegion*node =*nodePointer;
				path->getRegionsPath().push_back(node);
			}
		}else{
			delete path;
			return NULL;
		}
	}

	path->setCurrentRegion(path->getRegionsPath()[0]);
	return path;
}

bool NavmeshPath::recalculateLocalPath(){
	this;
	pointsInCurrentSection.clear();
	pointsIndexInCurrentSection.clear();
	if(getStartLocalPoint().getX()==-1) {
		int ix;
		int iy;
		getCurrentRegion()->getIndexOfCell(getStartPoint().getX(), getStartPoint().getY(),ix, iy);
		getStartLocalPoint().setXY(ix,iy);
	}

	if(getRegionsPath().size()==1) {
		int ix;
		int iy;
		getCurrentRegion()->getIndexOfCell(getEndPoint().getX(), getEndPoint().getY(),ix, iy);
		getEndLocalPoint().setXY(ix, iy);
	} else {
		int ix=0;
		int iy=0;
		if(passedRegionIndex==0){
			setCurrentRegion(getRegionsPath()[passedRegionIndex]);
			NavmeshRegion*nextRegion=getRegionsPath()[passedRegionIndex+1];
			hash_map<NavmeshRegion*,RegionConnection*>connectionMap=getCurrentRegion()->getConnections();
			RegionConnection*connection=connectionMap[nextRegion];
			vector<ConnectionPoint*>connectionPoints=connection->getConnectionPoints();
			int newConnectionPointIndex=rand() % connectionPoints.size();
			ConnectionPoint*point=connectionPoints[newConnectionPointIndex];
			int x=point->getPoint1().getX();
			int y=point->getPoint1().getY();
			getEndLocalPoint().setXY(x, y);
		}else if(passedRegionIndex<(int)(getRegionsPath().size()-1)){
			float xpos, ypos;
			getCurrentRegion()->getPositionOfCell(getEndLocalPoint().getX(), getEndLocalPoint().getY(), xpos, ypos);
			NavmeshRegion*newCurrentRegion=regionsPath[passedRegionIndex];
			setCurrentRegion(newCurrentRegion);
			int ix=0,iy=0;
			newCurrentRegion->getIndexOfCell(xpos, ypos, ix, iy);
			getStartLocalPoint().setXY(ix,iy);
			//navmeshManager->setCellWidthHeight(4);
			//navmeshManager->setStartDrawPoint(new vector3df(xpos-1.7,0, ypos-1.7));

			NavmeshRegion*nextRegion=regionsPath[passedRegionIndex+1];
			hash_map<NavmeshRegion*,RegionConnection*>connectionMap=getCurrentRegion()->getConnections();
			RegionConnection*connection=connectionMap[nextRegion];
			vector<ConnectionPoint*>connectionPoints=connection->getConnectionPoints();
			int newConnectionPointIndex=rand() % connectionPoints.size();
			ConnectionPoint*point=connectionPoints[newConnectionPointIndex];
			int x=point->getPoint1().getX();
			int y=point->getPoint1().getY();
			getEndLocalPoint().setXY(x, y);
			//newCurrentRegion->getPositionOfCell(x,y,xpos, ypos);
			//navmeshManager->setEndDrawPoint(new vector3df(xpos-1.7,0, ypos-1.7));
		}else if(passedRegionIndex==(regionsPath.size()-1)){
			float xpos, ypos;
			getCurrentRegion()->getPositionOfCell(getEndLocalPoint().getX(), getEndLocalPoint().getY(), xpos, ypos);
			NavmeshRegion*newCurrentRegion=regionsPath[passedRegionIndex];
			setCurrentRegion(newCurrentRegion);
			int ix=0,iy=0;
			newCurrentRegion->getIndexOfCell(xpos, ypos, ix, iy);
			getStartLocalPoint().setXY(ix,iy);
			getCurrentRegion()->getIndexOfCell(getEndPoint().getX(), getEndPoint().getY(), ix, iy);
			getEndLocalPoint().setXY(ix, iy);
			
			/*navmeshManager->setCellWidthHeight(4);
			navmeshManager->setStartDrawPoint(new vector3df(xpos-1.7,0, ypos-1.7));
			int endPointX=getEndPoint().getX();
			int endPointY=getEndPoint().getY();
			newCurrentRegion->getPositionOfCell(endPointX,endPointY,xpos, ypos);
			navmeshManager->setEndDrawPoint(new vector3df(xpos-1.7,0, ypos-1.7));*/
		}else{
			return false;
		}
	}

	GridSearch gsearcher(getCurrentRegion(),shared_ptr<Point>(new Point(getEndLocalPoint().getX(),getEndLocalPoint().getY())));
	vector<shared_ptr<Point>>result=gsearcher.compute(shared_ptr<Point>(new Point(getStartLocalPoint().getX(), getStartLocalPoint().getY())));
	if(result.size()>0){
		for(unsigned int i=0;i<result.size();i++){
			shared_ptr<Point>p=result[i];
			pointsIndexInCurrentSection.push_back(Point(p->getX(),p->getY()));
		}

		long start=GetTickCount();
		PathSmoother*smoother=new PathSmoother(currentRegion, &pointsIndexInCurrentSection);
		pointsIndexInCurrentSection=smoother->smooth();
		
		for(unsigned int i=0;i<pointsIndexInCurrentSection.size();i++){
			Point*solutionNode=&(pointsIndexInCurrentSection[i]);
			float xp, yp;
			currentRegion->getPositionOfCell(solutionNode->getX(), solutionNode->getY(), xp, yp);
			pointsInCurrentSection.push_back(vector3df(xp,0,yp));
		}
		long end=GetTickCount();
		printf("%d\n",(int)(end-start));
	}

	initialized=true;
	return true;
}


float lineLength(vector3df*p1, vector3df*p2) {
	return sqrt((p1->X - p2->X) * (p1->X - p2->X) + (p1->Z - p2->Z) * (p1->Z - p2->Z));
}

bool NavmeshPath::nextPathPoint(vector3df&newPosition, float length) {
	return nextPathPoint(newPosition, length, true);
}

bool NavmeshPath::nextPathPoint(vector3df&newPosition, float length, bool first) {
	if(!getInitialized()) {
		if(!recalculateLocalPath()){
			return false;
		}
		passedPointIndex=0;
	}
	
	vector3df*cp;
	vector3df*lp=&newPosition;
	bool found = false;
	for (unsigned int i = passedPointIndex; i < pointsInCurrentSection.size(); i++) {
		cp = &pointsInCurrentSection[i];
		float l = lineLength(&newPosition, cp);
		if (length <= l) {
			found = true;
			break;
		} else {
			length -= l;
		}

		lp = cp;
		passedPointIndex++;
	}

	if (!found || length == -1) {
		if(first==false){
			return false;
		}else{
			initialized=false;
			passedRegionIndex++;
			return nextPathPoint(newPosition, length, false);
		}
	}

	double angle = atan2(cp->Z - newPosition.Z, cp->X - newPosition.X);
	float x = (float)(cos(angle) * length + lp->X);
	float y = (float)(sin(angle) * length + lp->Z);
	newPosition.X=x;
	newPosition.Z=y;
	return true;
}

bool PathSmoother::isWalkable(int index1, int index2){
	Point a = (*path)[index1];
	Point b = (*path)[index2];
	return polygon->checkVisionLine(a.getX(), a.getY(), b.getX(), b.getY());
}

vector<Point> PathSmoother::smooth(){
	vector<Point>newPath;
	int start = 0;
	unsigned int nextPoint = 2;
	Point p=(*path)[start];
	newPath.push_back(p);
	bool cycle = true;
	while (cycle) {
		int size=path->size();
		for (int i = nextPoint; i < size; i++) {
			if(i==22){
				//TODO remove. Debug
				int x=5;x++;
			}
			if (isWalkable(start, nextPoint)) {
				nextPoint += 1;
			} else {
				newPath.push_back((*path)[nextPoint]);
				start = nextPoint;
				nextPoint += 3;
			}

			if (nextPoint >= path->size()) {
				newPath.push_back((*path)[path->size() - 1]);
				return newPath;
			}
		}

		if (nextPoint >= path->size()) {
			newPath.push_back((*path)[path->size() - 1]);
			return newPath;
		}
	}

	return newPath;
}